
package edu.archwood.frc2607.logomotion;

import com.sun.squawk.util.StringTokenizer;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStationLCD;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Timer;

public class LogomotionBot extends IterativeRobot
                           implements LogomotionConstants {

    private LogomotionDriveTrain driveTrain = null;
    private LogomotionArm theArm = null;
    private robovikingStick driveStick = null, gameStick = null;
    private LogomotionTelemetry dataLogger = null;
    private int loopCount = 0, autonStep;
    private Compressor theCompressor = null;
    private DriverStation ds = null;
    private Timer autonTimer = null;
private double previous_forward = 0;
private double previous_turn = 0;
private double previous_rotate = 0;
private double timeSinceUpdate = 0;
private byte autonMode = 0;
    public void robotInit() {

        System.out.println("Entering robotInit()");
        driveTrain = new LogomotionDriveTrain();
        driveStick = new robovikingStick(1);
        gameStick = new robovikingStick(2);
        ds = DriverStation.getInstance();
        theArm = new LogomotionArm();
        theCompressor = new Compressor(compressorSwitchDI, compressorRelay);
        theCompressor.stop();

        dataLogger = new LogomotionTelemetry();
        dataLogger.addLoggedObject(this);
        dataLogger.addLoggedObject(driveTrain);
        dataLogger.addLoggedObject(theArm);
        autonTimer = new Timer();
        System.out.println("Done robotInit()");
    }

    boolean writeMsg;
    /**
     * @deprecated
     */
public void selfDestruct()
    {
            
    }
    public void autonomousInit() {
        loopCount = 0;
        autonStep = 0;
        autonTimer.start();
        autonTimer.reset();
        dataLogger.setLoggingInterval(30);
        writeMsg = true;
        System.out.println("autonomousInit()");
    }

// 452, 312 for top side,  move elbow to 481 to hang, then back to 322,216 and reverse

// 590, 550 for top middle
    public void autonomousPeriodic() {
//TODO: add switch for auton modes
        switch (autonMode)
        {

            case 0:
        switch (autonStep) {
            case 0:
                if (driveTrain.driveToZone(-.15) || autonTimer.get() >= 5) {
                    ++autonStep;
                    autonTimer.reset();
                }
                break;
            case 1:
                driveTrain.mecanumDrive(0.0,0.0,0.0);
                if (theArm.gotoPosition(590,550)) {
                    ++autonStep;
                    autonTimer.reset();
                }
                break;
            case 2:
                theArm.setElbowSpeed(0.0);
                theArm.setShoulderSpeed(0.0);
                driveTrain.mecanumDrive(-.02,0.0,0.0);
                if (autonTimer.get() >= 1.5) {
                    ++autonStep;
                    autonTimer.reset();
                }
                break;
            case 3:
                if (theArm.gotoPosition(590,580)) {
                    ++autonStep;
                    autonTimer.reset();
                }
                break;
            case 4:
                theArm.openClaw();
                if (autonTimer.get() >= .7) {
                    ++autonStep;
                    autonTimer.reset();
                }
                break;
            case 5:
                driveTrain.mecanumDrive(.2,0.0,0.0);
                theArm.gotoPosition(135,20);
                if (autonTimer.get() >= 3) {
                    ++autonStep;
                    autonTimer.reset();
                }
                break;
            case 6:
                driveTrain.mecanumDrive(0.0,0.0,0.0);
                theArm.setElbowSpeed(0.0);
                theArm.setShoulderSpeed(0.0);
                break;
        }
        break;
            case 1:
                break;
        }
    }
/*
        public void autonomousPeriodic() {
        
        // assume we start on the straight line.  Follow it and score on
        // middle peg
        boolean endOfLine;
        if (writeMsg) {
            System.out.println("autonStep: " + autonStep);
            writeMsg = false;
        }
        switch (autonStep) {
            //follow line for 3 seconds
            case 0:
                driveTrain.followLine(.09, .05);
                if (autonTimer.get() >= 3) { autonTimer.reset(); ++autonStep; writeMsg = true;}
                break;
           // continue following line but also move arm into position
            case 1:
                driveTrain.followLine(.09, .05);
                //160, 80
                theArm.gotoPosition(160,80);
                if (autonTimer.get() >= 3) {
                    autonTimer.reset();
                    ++autonStep;
                    writeMsg = true;
                }
                break;
           // stop the arm motors (just in case it's bouncing)
            case 2:
                theArm.setElbowSpeed(0.0);
                theArm.setShoulderSpeed(0.0);
                driveTrain.followLine(.08, .05);
                if (autonTimer.get() >= 2) {
                    autonTimer.reset();
                    ++autonStep;
                    writeMsg = true;
                }
                break;
            case 3:
                driveTrain.mecanumDrive(0.0, 0.0, 0.0);
                if (theArm.gotoPosition(160, 182) || autonTimer.get() >= 2) {
                    autonTimer.reset();
                    ++autonStep;
                    writeMsg = true;
                }
                break;
            case 4:
                theArm.setElbowSpeed(0.0);
                theArm.setShoulderSpeed(0.0);
                theArm.openClaw();
                if (autonTimer.get() >= 1) {
                    autonTimer.reset();
                    ++autonStep;
                    writeMsg = true;
                }
                break;
            case 5:
                if(theArm.gotoPosition(140, 180) || autonTimer.get() >= 2) {
                    autonTimer.reset();
                    ++autonStep;
                    writeMsg = true;
                }
                break;
            case 6:
                driveTrain.mecanumDrive(0.2, 0.0, 0.0);
                if (theArm.gotoPosition (135, 20) || autonTimer.get() >= 2) {
                    autonTimer.reset();
                    ++autonStep;
                    writeMsg = true;
                }
                break;
            case 7:
               driveTrain.mecanumDrive(0.0, 0.0, 0.0);
               theArm.setElbowSpeed(0.0);
               theArm.setShoulderSpeed(0.0);
               break;
        }
    }
*/


    public void disabledInit() {
        System.out.println("Entering disabledInit()");
        theCompressor.stop();
        driveTrain.mecanumDrive(0.0,0.0,0.0);
        loopCount = 0;
        dataLogger.setLoggingInterval(2000);
        dataLogger.startLogging();
        System.out.println("Leaving disabledInit()");
    }

    public void disabledPeriodic() {

    }

    public void teleopInit() {
        System.out.println("Entering teleopInit()");
        loopCount = 0;
        driveTrain.resetGyro();
        driveTrain.mecanumDrive(0.0,0.0,0.0);
        theCompressor.stop();
        dataLogger.setLoggingInterval(30);
        System.out.println("Leaving teleopInit()");
    }

    public void teleopPeriodic() {
      
        if (gameStick.getButtonToggle(6)){
            if (theCompressor.enabled())
                theCompressor.stop();
            else theCompressor.start();
        }

/*
             if (driveStick.getRawButton(1))
                turn = .5;
             else if (driveStick.getRawButton(3))
                 turn = -.5;

             if (driveStick.getRawButton(2))
                 forward = .5;
             else if (driveStick.getRawButton(4))
                 forward = -.5;
*/
        if (!gameStick.getRawButton(8) && !gameStick.getRawButton(7)) {
            if (gameStick.getRawButton(4)) theArm.setShoulderSpeed(-.5);
            else if (gameStick.getRawButton(2)) theArm.setShoulderSpeed(.5);
            else theArm.setShoulderSpeed(0.0);

            if (gameStick.getRawButton(1)) theArm.setElbowSpeed(-.4);
        	else if (gameStick.getRawButton(3)) theArm.setElbowSpeed(.4);
            else theArm.setElbowSpeed(0.0);
        } else {
            if (gameStick.getRawButton(8))
            theArm.gotoPosition(150, 72);
            else
            {
                if (gameStick.getRawButton(7))
                theArm.gotoPosition(133, 19);
            }
        }

        double forward = driveStick.getY();
        double turn = -driveStick.getX();
        double rotate = -driveStick.getRawAxis(3);
         if (forward > -0.01 && forward < 0.01) forward = 0;
            if (turn > -0.01 && turn < 0.01) turn = 0;
            if (rotate > -0.01 && rotate < 0.01) rotate = 0;

        if (rotate!=previous_rotate||turn!=previous_turn||forward!=previous_turn)//Only drive if nothing has changed
        {
        driveTrain.mecanumDrivex(forward, turn, rotate);
        }
        else
        {//If nothing has changed...
            timeSinceUpdate+=.02;//Increase our timer
            if (timeSinceUpdate > .1)//After a tenth of a second...
            {
                driveTrain.mecanumDrivex(forward, turn, rotate);//Drive anyway
                timeSinceUpdate = 0;//And reset the timer
            }
        }
previous_forward = forward;
previous_turn = turn;
previous_rotate = rotate;
        if (gameStick.getButtonToggle(5)) {
            theArm.toggleClaw();
        }

         DriverStationLCD.getInstance().println(DriverStationLCD.Line.kMain6, 1,
                                                theArm.getPotValues());
/*
         StringTokenizer st = driveTrain.motorSPs();
         DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser2, 1,
                                        st.nextToken());
         DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser3, 1,
                                        st.nextToken());
*/
         DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser2, 1,
                                        driveTrain.getSwitches());

         StringTokenizer st = driveTrain.motorPVs();
         DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser4, 1,
                                        st.nextToken());
         DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser5, 1,
                                        st.nextToken());
         String msg = "Gyro: " + String.valueOf(driveTrain.getGyroAngle()).substring(0,3);
         msg += " Temp: " + String.valueOf(driveTrain.getCurTemp()).substring(0,3);
         DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser6, 1,
                                        msg);
         DriverStationLCD.getInstance().updateLCD();

    }

    public String getTelemetryHeader() {
        return "DS Packet,Mode,DriveX,DriveY,DriveZ,Drive Btns,Game Btns";
    }

    public String getTelemetryData() {
        String msg = String.valueOf(ds.getPacketNumber()) + ",";    // DS Packet
        msg += (ds.isAutonomous() ? "Auto" : "Teleop");
        msg += (ds.isDisabled() ? "Disabled" : "Enabled") + ",";    // Mode
        msg += Double.toString(driveStick.getX()) + ",";             // DriveX
        msg += Double.toString(driveStick.getY()) + ",";             // DriveY
        msg += Double.toString(driveStick.getRawAxis(3)) + ",";      // DriveZ
        msg += Integer.toBinaryString(ds.getStickButtons(1)) + ","; // Drive Btns
        msg += Integer.toBinaryString(ds.getStickButtons(2));       // Game Btns
        return msg;
    }
}
